# _*_ coding: utf-8 _*_
# @Author   : Wei Yue
# @Time     : 2023-05-17 13:14
# @Function : 鲁棒微分器类

from motorCalibration.motorContanst import *


class RobustDiff:
    x_1 = 0
    x_2 = 0
    u_1 = 0
    u_2 = 0
    Ts = 0
    a = 0

    def __init__(self, Ts, a):
        self.a = a
        self.Ts = Ts

    def diff(self, value_next):
        # 输入值经过鲁棒微分器 输出微分项 避免干扰引起的尖刺
        k1 = sign(self.x_1 - value_next)
        m1 = abs(self.x_1 - value_next)

        self.x_2 = self.Ts * (self.u_1 - self.a * math.sqrt(m1) * k1) + self.x_1
        self.u_2 = - self.Ts * k1 + self.u_1
        U = self.u_1 - self.a * math.sqrt(m1) * k1
        self.x_1 = self.x_2
        self.u_1 = self.u_2

        return U
